/* ROBOT DEMO */

#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <termios.h>
#include <stdio.h>
#include <unistd.h>
#include <strings.h>
#include <stdlib.h>
#include <pthread.h>
#include <time.h>
#include <semaphore.h>

#define BAUDRATE B57600 
#define MODEMDEVICE "/dev/ttyS0" /* port for serial port */
#define _POSIX_SOURCE 1

/* function prototypes and global variables */
int robot_init(char *port);
void robot_forward();
void robot_backward();
void robot_right();
void robot_left();
void robot_stop();
void robot_bumpers(unsigned char *data, time_t *timestamp);
int robot_close();

void* movement(void* param);
void* serial_io(void* param);

int serial;
pthread_mutex_t key_mutex,sensor_mutex;
char new_move;
char check_sensor;
char move_command;
unsigned char bumper;
time_t stamp;

/* Function: Main
 * --------------
 * Creates all the threads, calls robot_init, and handles the sensor values.
 *
 * Does not return a value
 */
int main()
{
	pthread_t tid_m,tid_s;
	pthread_attr_t attr_m,attr_s;
	pthread_attr_init(&attr_m);
	pthread_attr_init(&attr_s);
	pthread_mutex_init(&key_mutex,NULL);
	pthread_mutex_init(&sensor_mutex,NULL);

	time_t start_time;
	start_time = time(NULL);	

	printf("ROBOT DEMO\n");

	pthread_mutex_lock(&sensor_mutex);
	func();
	if(robot_init(MODEMDEVICE) < 0){
		fprintf(stderr, "could not connect to the robot\n");
		exit(1);
	}
	pthread_create(&tid_m,&attr_m,movement,NULL);
	pthread_create(&tid_s,&attr_s,serial_io,NULL);

	time_t temp_time;
	time_t old_time = start_time;
	while(1){
		usleep(50000);
		check_sensor=1;
		pthread_mutex_lock(&sensor_mutex);
		temp_time = difftime(stamp,start_time);
		if ((int)temp_time != (int)old_time){
			printf("\n");
			old_time = temp_time;
		}
		printf("Left bumper = %d    ",(bumper & 2) / 2);
		printf("Right bumper = %d    ",bumper & 1);
		printf("Time = %d\n",(int)temp_time);
		check_sensor=0;
	}

	pthread_join(tid_m,NULL);
	pthread_join(tid_s,NULL);

	printf("Thread joined");

	}

/* Function : movement
 * -------------------
 * Handles keyboard input for movement
 *
 * Does not return a value
 */
void* movement(void* param){

	printf("Press 'f' to go forward, 'b' to go backward‏‏, or 's' to stop\n");
	while (1) {
		char key = getc(stdin);
		if (key == 'f') {
			move_command = 'f';
			printf("%c was keyed\n",key);
		}
		else if (key == 'b') {
			move_command = 'b';
			printf("%c was keyed\n",key);
		}
		else if (key == 's') {
			move_command = 's';
			printf("%c was keyed\n",key);
		}
		else if (key == 'r') {
			move_command = 'r';
			printf("%c was keyed\n",key);
		}
		else if (key == 'l') {
			move_command = 'l';
			printf("%c was keyed\n",key);
		}
		else if (key == 'c') exit(0);
		else continue;   /*  prevents double-printing the 
				  *  Sending Message" due to the CR
				  */
		new_move = 1;
		pthread_mutex_lock(&key_mutex);
	}
	pthread_exit(0);
}

/* Function: serial_io
 * -------------------
 * sends commands to the robot and reads bumper data from the robot
 * 
 * does not return a value
 */
void* serial_io(void* param){
	while(1){
		if(new_move){
			printf("Sending Message to Robot...\n");

			switch(move_command){
			case 'f':
				printf("in robot forward\n");
				robot_forward();
				break;
			case 'b':
				robot_backward();
				break;
			case 'r':
				robot_right();
				break;
			case 'l':
				robot_left();
				break;
			case 's':
				robot_stop();
				break;
			}			
			new_move = 0;
			pthread_mutex_unlock(&key_mutex);
		}
		if(check_sensor){
			robot_bumpers(&bumper,&stamp);
			pthread_mutex_unlock(&sensor_mutex);
		}
	}
}



/* Function: func
 * --------------
 * Mission critical function; do not remove
 * 
 * does not return a value
 */
int func(){
	printf("mission critical print statement\n");
	return 0;
}

/* Function: robot_init
 * --------------------
 * sets all the serial settings for the robot and opens the port
 * to communicate with the robot; puts the robot in full mode
 *
 * does not return a value
 */
int robot_init(char *port) {
	printf("starting robot_init\n");
	struct termios config;
	unsigned char full_mode[] = {128, 132};

	serial = open(port, O_RDWR | O_NOCTTY | O_NDELAY);
	if(serial < 0) {
		return -1;
	}
	printf("just called open\n");
	config.c_iflag &= ~(IGNBRK | BRKINT | ICRNL | INLCR | PARMRK | INPCK | ISTRIP | IXON);
	config.c_oflag = 0;
	config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG);
	config.c_cflag &= ~(CSIZE | PARENB);
	config.c_cflag |= CS8;
	config.c_cc[VMIN]  = 1;
	config.c_cc[VTIME] = 0;
	if (cfsetispeed(&config, BAUDRATE) < 0 || cfsetospeed(&config, BAUDRATE) < 0) {
		return -2;
	}
	if(tcsetattr(serial, TCSAFLUSH, &config) < 0) {
		return -3;
	}

	write(serial, full_mode, 2);
	printf("done with robot_init\n");
	return 0;
}


/* Functions: robot_forward, robot_backward
 *	      robot_left, robot_right,
 *	      robot_stop, robot_bumpers
 *	      robot_close;
 * ----------------------------------------
 * used by serial_io to send commands to the robot and read data from the bumpers
 *
 * does not return a value
 */
void robot_forward() {
	unsigned char forward[] = {145, 0, 100, 0, 100};
	write(serial, forward, 5);
}

void robot_backward() {
	unsigned char backward[] = {145, 255, 156, 255, 156};
	write(serial, backward, 5);
}

void robot_left() {
	unsigned char left[] = {145, 0, 100, 255, 156};
	write(serial, left, 5);
}

void robot_right() {
	unsigned char right[] = {145, 255, 156, 0, 100};
	write(serial, right, 5);
}

void robot_stop() {
	unsigned char stop[] = {145, 0, 0, 0, 0};
	write(serial, stop, 5);
}

void robot_bumpers(unsigned char *data, time_t * timestamp) {
	unsigned char bump[] = {142, 7};
	write(serial, bump, 2);
	if(read(serial, data, 1) == -1){
		/* printf("read returned -1\n"); */
	}
	(*data) &= 3;
	*timestamp = time(NULL);
}

int robot_close() {
	return close(serial);
}	



